
/**
 * 头文件包含顺序：
 * 本代码文件的头文件、C库函数头文件、系统头文件、本项目其他模块头文件
 */
#include "gige.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <math.h>
#include "MvCameraControl.h"
#include "mpi_venc.h"
#include "hi_common.h"
#include "hi_comm_video.h"
#include "hi_comm_sys.h"
#include "mpi_sys.h"
#include "hi_comm_vb.h"
#include "mpi_vb.h"
#include "hi_comm_vpss.h"
#include "mpi_vpss.h"
#include "mpi_vgs.h"

#include "ringbuf.h"
#include "util.h"
#include "modbus-srv.h"
#include "ctrl.h"



/**
 * 不需要其他模块知道的宏定义不要暴露在头文件里
 */

/**
 * The number of MV_CC_GetImageBuffer error
 * Exit app if exceed this value
 */
#define GIGE_ERROR_LIMIT 100

typedef struct GigeData {
	int fps;        // 帧率
    int exposure;   // 曝光时间           
	int height;     // 帧高
	int width;      // 帧宽
	int frame_size; // 帧数据大小 height*width
	void* handle;   // sdk句柄
	unsigned int payload_size; // 帧数据大小
	pthread_t thr_id;   // 线程id
    int run;    // 线程运行标记
    ringbuf_t rb;  // 帧队列
}GigeData;

static VIDEO_FRAME_INFO_S video_frame;
static GigeData gigeobj; // 模块全局变量

struct hiDEMO_MEMBUF_S
{
    VB_BLK  hBlock;
    VB_POOL hPool;
    HI_U32  u32PoolId;
	HI_U32 u32BlkSize;
    HI_U64 u64PhyAddr;
    HI_U64 u64VirAddr;
    HI_S32  s32Mdev;
} g_stMem;

static int globa_time = 0; // 全局帧时间，传给vpss的时间需要递增，出现重连gige的时候需要保存时间信息。

/**
 * 仅在本文件使用的函数使用 static 限制函数作用域
 */

static int _gige_init();

static int _gige_start_grab();

/**
 * @brief 创建相机句柄
 * 
 * @param handle 
 * @return int 
 */
static int _create_gige_handle(void** handle);

static void _set_optimal_packsize(void* handle);

static bool _print_device_info(MV_CC_DEVICE_INFO* pstMVDevInfo);

HI_S32 gige_release_vb()
{
    if(HI_NULL != g_stMem.u64VirAddr)
    {
        HI_MPI_SYS_Munmap((HI_VOID*)g_stMem.u64VirAddr, g_stMem.u32BlkSize);
        g_stMem.u64PhyAddr = HI_NULL;
    }
    if(VB_INVALID_HANDLE != g_stMem.hBlock)
    {
        HI_MPI_VB_ReleaseBlock(g_stMem.hBlock);
        g_stMem.hBlock = VB_INVALID_HANDLE;
    }

    if (VB_INVALID_POOLID != g_stMem.hPool)
    {
        HI_MPI_VB_DestroyPool(g_stMem.hPool);
        g_stMem.hPool = VB_INVALID_POOLID;
    }
	
    return HI_SUCCESS;
}

HI_S32 gige_alloc_vb(int nImageWidth, int nImageHeight, VIDEO_FRAME_INFO_S* pstFrmInfo)
{
	//HI_S32 ret = -1;
    VB_POOL_CONFIG_S stVbPoolCfg;
    HI_U32 u32LumaSize = 0, u32ChrmSize = 0;
	if (pstFrmInfo == NULL){
		return HI_FAILURE;
	}
	// Although we need w*h*1.5 alloc more buffer does not impact
	g_stMem.u32BlkSize = nImageWidth*nImageHeight*2;
    memset(&stVbPoolCfg, 0, sizeof(VB_POOL_CONFIG_S));
    stVbPoolCfg.u64BlkSize  = g_stMem.u32BlkSize;
    stVbPoolCfg.u32BlkCnt   = 1;
    stVbPoolCfg.enRemapMode = VB_REMAP_MODE_NONE;
	g_stMem.hPool = HI_MPI_VB_CreatePool(&stVbPoolCfg);
	if ( VB_INVALID_POOLID == g_stMem.hPool)
	{
		__log_mesg("create vb err\n");
		return HI_FAILURE;
	}

	g_stMem.hBlock = HI_MPI_VB_GetBlock(g_stMem.hPool,g_stMem.u32BlkSize,HI_NULL);
	if (VB_INVALID_HANDLE ==g_stMem.hBlock )
	{
		__log_mesg("get vb block err\n");
		gige_release_vb();
		return HI_FAILURE;
	}

	g_stMem.u64PhyAddr = HI_MPI_VB_Handle2PhysAddr(g_stMem.hBlock);
	if (HI_NULL == g_stMem.u64PhyAddr)
	{
		__log_mesg("blk to physaddr err\n");
		gige_release_vb();
		return HI_FAILURE;
	}

	g_stMem.u64VirAddr = (HI_U64)HI_MPI_SYS_Mmap(g_stMem.u64PhyAddr,g_stMem.u32BlkSize);
    if (g_stMem.u64VirAddr == 0)
    {
        __log_mesg("Mem dev may not open\n");
        gige_release_vb();
        return HI_FAILURE;
    }
	memset(pstFrmInfo,0,sizeof(VIDEO_FRAME_INFO_S));
    pstFrmInfo->stVFrame.u64PhyAddr[0] = g_stMem.u64PhyAddr;

	u32LumaSize = nImageWidth *  nImageHeight;
	u32ChrmSize = nImageWidth *  nImageHeight >> 2;
	
    pstFrmInfo->stVFrame.u64PhyAddr[0] = g_stMem.u64PhyAddr;
    pstFrmInfo->stVFrame.u64PhyAddr[1] = pstFrmInfo->stVFrame.u64PhyAddr[0] + u32LumaSize;
	pstFrmInfo->stVFrame.u64PhyAddr[2] = pstFrmInfo->stVFrame.u64PhyAddr[1] + u32ChrmSize;

    pstFrmInfo->stVFrame.u64VirAddr[0] = g_stMem.u64VirAddr;
    pstFrmInfo->stVFrame.u64VirAddr[1] = (HI_U64) pstFrmInfo->stVFrame.u64VirAddr[0] + u32LumaSize;
	pstFrmInfo->stVFrame.u64VirAddr[2] = (HI_U64) pstFrmInfo->stVFrame.u64VirAddr[1] + u32ChrmSize;

    pstFrmInfo->stVFrame.u32Width  = nImageWidth;
    pstFrmInfo->stVFrame.u32Height = nImageHeight;
	pstFrmInfo->stVFrame.u32Stride[0] = nImageWidth;
	pstFrmInfo->stVFrame.u32Stride[1] = nImageWidth;

    pstFrmInfo->stVFrame.enField = VIDEO_FIELD_FRAME;
    pstFrmInfo->stVFrame.enCompressMode = COMPRESS_MODE_NONE;
    //pstFrmInfo->stVFrame.enPixelFormat = PIXEL_FORMAT_YVU_SEMIPLANAR_420;
    pstFrmInfo->stVFrame.enPixelFormat = PIXEL_FORMAT_YUV_400;
    //pstFrmInfo->stVFrame.u64PTS = 0;
    pstFrmInfo->stVFrame.u32TimeRef = globa_time;
    pstFrmInfo->u32PoolId = g_stMem.hPool;

	return HI_SUCCESS;
}

bool _print_device_info(MV_CC_DEVICE_INFO* pstMVDevInfo)
{
    if (NULL == pstMVDevInfo) {
        __log_mesg("The Pointer of pstMVDevInfo is NULL!\n");
        return false;
    }
    if (pstMVDevInfo->nTLayerType == MV_GIGE_DEVICE) {
        int nIp1 = ((pstMVDevInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24);
        int nIp2 = ((pstMVDevInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16);
        int nIp3 = ((pstMVDevInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8);
        int nIp4 = (pstMVDevInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff);

        // ch:打印当前相机ip和用户自定义名字 | en:print current ip and user defined name
        __log_mesg("Gige camera ip: %d.%d.%d.%d\n" , nIp1, nIp2, nIp3, nIp4);
        //__log_mesg("UserDefinedName: %s\n\n" , pstMVDevInfo->SpecialInfo.stGigEInfo.chUserDefinedName);
    }
    else {
        __log_mesg("Not support.\n");
    }
    return true;
}

void __stdcall gige_exception(unsigned int nMsgType, void* pUser)
{   
	int retry = 5;
    set_gige_exception();
    __log_mesg("Device disconnect! Code=[0x%x].\n", nMsgType);
    gige_close();
    __log_mesg("Enter retry loop.\n");
	do{
        __log_mesg("Reconnect %d times ...\n", retry);
        if(gige_open()) {
            if (--retry < 1) {
                // 重启工业相机,退出程序
                ctrl_gige_reboot();
                exit(-1);
            }
            __log_mesg("Connect fail. Sleep 5s. Try again.\n");
			sleep(5);
			continue;
		}
		__log_mesg("Reconnect success.\n");
        set_gige_normal();
		return; 
	} while (1);
    
	__log_mesg("Reconnect fail.\n");
}

int _create_gige_handle(void** handle) 
{
    MV_CC_DEVICE_INFO_LIST stDeviceList;
    memset(&stDeviceList, 0, sizeof(MV_CC_DEVICE_INFO_LIST));
    int nRet = MV_CC_EnumDevices(MV_GIGE_DEVICE|MV_USB_DEVICE, &stDeviceList);
    if (MV_OK != nRet) {
        __log_mesg("Enum Devices fail! nRet [0x%x]\n", nRet);
        return EXIT_FAILURE;
    }
    if (stDeviceList.nDeviceNum > 0) {
        for (unsigned int i = 0; i < stDeviceList.nDeviceNum; i++)
        {
            MV_CC_DEVICE_INFO* pDeviceInfo = stDeviceList.pDeviceInfo[i];
            if (NULL == pDeviceInfo)
            {
                break;
            } 
            _print_device_info(pDeviceInfo);            
        }  
    } else {
        __log_mesg("Find No Gige Devices!\n");
        return EXIT_FAILURE;
    }

    // ch:选择设备并创建句柄 | en:Select device and create handle
    nRet = MV_CC_CreateHandleWithoutLog(handle, stDeviceList.pDeviceInfo[0]);
    if (MV_OK != nRet)
    {
        __log_mesg("Create Handle fail! nRet [0x%x]\n", nRet);
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}

static void _set_optimal_packsize(void* handle)
{
    int nPacketSize = MV_CC_GetOptimalPacketSize(handle);
        if (nPacketSize > 0)
        {
            int nRet = MV_CC_SetIntValue(handle,"GevSCPSPacketSize",nPacketSize);
            if(nRet != MV_OK)
            {
                __log_mesg("Warning: Set Packet Size fail nRet [0x%x]!", nRet);
            }
        }
        else
        {
            __log_mesg("Warning: Get Packet Size fail nRet [0x%x]!", nPacketSize);
        }
}

int _init_gigeobj()
{
    MVCC_INTVALUE stParam[3];
    MVCC_FLOATVALUE stfParam[2];

    memset(&stParam, 0, sizeof(MVCC_INTVALUE));
    if (MV_CC_GetIntValue(gigeobj.handle, "PayloadSize", &stParam[0]) ||
        MV_CC_GetIntValue(gigeobj.handle, "Width", &stParam[1]) ||
        MV_CC_GetIntValue(gigeobj.handle, "Height", &stParam[2]) ||
        MV_CC_GetFloatValue(gigeobj.handle, "ResultingFrameRate", &stfParam[0]) ||
        MV_CC_GetFloatValue(gigeobj.handle, "ExposureTime", &stfParam[1])) 
    {
        __log_mesg("Get camera info fail.\n");
        return EXIT_FAILURE;
    }
    gigeobj.payload_size = stParam[0].nCurValue;
    gigeobj.width = stParam[1].nCurValue;
    gigeobj.height = stParam[2].nCurValue;
    gigeobj.frame_size = gigeobj.width * gigeobj.height;
    gigeobj.fps = roundf(stfParam[0].fCurValue);
    gigeobj.exposure = roundf(stfParam[1].fCurValue);
    
    __log_mesg("Gige origin property=>payload = %d width = %d height = %d fps = %.2f exposure = %.2f\n", 
    gigeobj.payload_size, 
    gigeobj.width, 
    gigeobj.height, 
    stfParam[0].fCurValue,
    stfParam[1].fCurValue);

    gige_alloc_vb(gigeobj.width, gigeobj.height, &video_frame);
    gigeobj.rb = ringbuf_new(8 * gigeobj.frame_size);
    return EXIT_SUCCESS;
}

/**
 * Find gige device
 * Create handle
 * Open device
 * Set and get camera info (h,w,payload,triggermode,SCPSPacketSize and exception callback)
 * Start grabbing
 * If one of these steps fails , _gige_init failed.
 */
int _gige_init()
{
	int nRet = -1;
    memset(&gigeobj, 0x00, sizeof(gigeobj));

    if (_create_gige_handle(&gigeobj.handle)) {
        __log_mesg("Create gige handle failed.\n");
        return EXIT_FAILURE;
    }

    // ch:打开设备 | en:Open device
    nRet = MV_CC_OpenDevice(gigeobj.handle, MV_ACCESS_Exclusive, 0);
    if (MV_OK != nRet)
    {
        __log_mesg("Open Device fail! nRet [0x%x]\n", nRet);
        return EXIT_FAILURE;
    }

    // ch:探测网络最佳包大小(只对GigE相机有效) | en:Detection network optimal package size(It only works for the GigE camera)
    _set_optimal_packsize(gigeobj.handle);

    // ch:设置触发模式为off | en:Set trigger mode as off
    nRet = MV_CC_SetEnumValue(gigeobj.handle, "TriggerMode", 0);
    if (MV_OK != nRet)
    {
        __log_mesg("Set Trigger Mode fail! nRet [0x%x]\n", nRet);
        return EXIT_FAILURE;
    }

    if (_init_gigeobj()) {
        __log_mesg("Init gige object failed.\n");
        return EXIT_FAILURE;
    }
    
    nRet = MV_CC_RegisterExceptionCallBack(gigeobj.handle, gige_exception, NULL);
    if (MV_OK != nRet)
    {
        __log_mesg("MV_CC_RegisterExceptionCallBack fail! nRet [%x]\n", nRet);
        return EXIT_FAILURE;
    }
   
    __log_mesg("Gige camera connect succeed\n");
	
    // ch:开始取流 | en:Start grab image
    nRet = MV_CC_StartGrabbing(gigeobj.handle);
    if (MV_OK != nRet)
    {
        __log_mesg("Start Grabbing fail! nRet [0x%x]\n", nRet);
        return EXIT_FAILURE;
    }
	return EXIT_SUCCESS;
}

int gige_push_to_ringbuf(MV_FRAME_OUT* frame)
{
    char* head = 0;
    head = (char*)ringbuf_memcpy_into(gigeobj.rb,frame->pBufAddr, gigeobj.frame_size);
    return head?  EXIT_SUCCESS:EXIT_FAILURE;
}

bool gige_send_to_vpss(MV_FRAME_OUT* frame)
{
    globa_time += 2; 
    video_frame.stVFrame.u32TimeRef = globa_time;
    //uint64_t timeref = frame->stFrameInfo.nDevTimeStampHigh;
    //timeref = (timeref << 32) | frame->stFrameInfo.nDevTimeStampLow;
   // video_frame.stVFrame.u64PTS = timeref; 
	memcpy((void*)g_stMem.u64VirAddr, frame->pBufAddr, gigeobj.frame_size);
	if (HI_MPI_VPSS_SendFrame(1, 0, &video_frame, 5000) != HI_SUCCESS)
	{
		__log_mesg("Sample_Com_SendYUV Failed!\n");
	}
    return true;
}

void* gige_grab_loop(void *arg)
{
	int nRet = MV_OK;
    MV_NETTRANS_INFO stNetTransInfo;
    //uint64_t frame_id = 0;
    MV_FRAME_OUT stOutFrame;
    int errcnt = 0;
    memset(&stOutFrame, 0, sizeof(MV_FRAME_OUT));
    while(gigeobj.run)
    {
        nRet = MV_CC_GetImageBuffer(gigeobj.handle, &stOutFrame, 1000);
        if (nRet == MV_OK)
        {
            errcnt = 0;
            //frame_id++;
            usleep(20);
            gige_push_to_ringbuf(&stOutFrame);
            gige_send_to_vpss(&stOutFrame);
            set_gige_normal();
            //if ((frame_id % 2500) == 0) {
            //    nRet = MV_GIGE_GetNetTransInfo(gigeobj.handle, &stNetTransInfo);
            //    if (MV_OK == nRet){
            //        __log_mesg("nNetRecvFrameCount [%u] nThrowFrameCount [%d]"
            //        "nRequestResendPacketCount [%lu] nResendPacketCount [%lu]\n", 
            //        stNetTransInfo.nNetRecvFrameCount,
            //        stNetTransInfo.nThrowFrameCount,
            //        stNetTransInfo.nRequestResendPacketCount,
            //        stNetTransInfo.nResendPacketCount);
            //    }
            //}
        }
        else
        {
            /**
             * @brief 如果连续GIGE_ERROR_LIMIT帧出现MV_CC_GetImageBuffer错误退出程序
             *       
             */
            if (errcnt++ > GIGE_ERROR_LIMIT) {
                set_gige_exception();
                __log_mesg("Grab gige stream error. Reboot gige camera.\n");
                ctrl_gige_reboot();
                exit(-1);
            }
            __log_mesg("No data[0x%x]\n", nRet);
        }
        if(NULL != stOutFrame.pBufAddr)
        {
            nRet = MV_CC_FreeImageBuffer(gigeobj.handle, &stOutFrame);
            if(nRet != MV_OK)
            {
                __log_mesg("Free Image Buffer fail! nRet [0x%x]\n", nRet);
            }
        }
    }
    return 0;
}

int _gige_start_grab()
{
    if (gigeobj.run){
		__log_mesg("Gige grabbing thread already started.\n");
	} else {
        gigeobj.run = 1;
        if(pthread_create(&gigeobj.thr_id, 0, gige_grab_loop, 0))
        {
            gigeobj.run = 0;
            __log_mesg("pthread_create gige_grab_thread error.\n");
        	return EXIT_FAILURE;
        }
    }
    return EXIT_SUCCESS;
}

int gige_open()
{ 
    if (_gige_init()) {
        return EXIT_FAILURE;
    }
    if (_gige_start_grab()) {
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}

int gige_close()
{
    int ret = MV_OK;
    gigeobj.run = 0; 
    if(gigeobj.thr_id)
	{
		if (pthread_join(gigeobj.thr_id, NULL))
		{
			__log_mesg("stream_thr pthread_join error: ret=%d !!!\n", ret);
            goto Err;
		}
        //gigeobj.thr_id = 0;
	}

    ret = MV_CC_StopGrabbing(gigeobj.handle);
    if (MV_OK != ret)
    {
        __log_mesg("Stop Grabbing fail! nRet [0x%x]\n", ret);
        goto Err;
    }
    // Close device
    ret = MV_CC_CloseDevice(gigeobj.handle);
    if (MV_OK != ret)
    {
        __log_mesg("ClosDevice fail! nRet [0x%x]\n", ret);
        goto Err;
    }

    // Destroy handle
    ret = MV_CC_DestroyHandle(gigeobj.handle);
    if (MV_OK != ret)
    {
        __log_mesg("Destroy Handle fail! nRet [0x%x]\n", ret);
        goto Err;
    }
Err:
    ringbuf_free(gigeobj.rb);
    gige_release_vb();
    return EXIT_SUCCESS;
}

int gige_is_opened()
{
    if (gigeobj.handle) return 1;
    return 0;
}

int gige_get_fps()
{
    return gigeobj.fps;
}

int gige_set_fps(int fps)
{
    if (gige_fps_manual_control_enable(true)) {
        return EXIT_FAILURE;
    }
    float ffps = fps;
    int nRet = MV_CC_SetFloatValue(gigeobj.handle, "AcquisitionFrameRate", ffps);
    if (MV_OK != nRet)
    {
        __log_mesg("error: gige_set_fps fail [%x]\n", nRet);
        return EXIT_FAILURE;
    }
    gigeobj.fps = fps;
    return EXIT_SUCCESS;
}

int gige_get_exposure()
{
    return gigeobj.exposure;
}

int gige_set_exposure(int exposure)
{
    float fexposure = exposure;
    int nRet = MV_CC_SetFloatValue(gigeobj.handle, "ExposureTime", fexposure);
    if (MV_OK != nRet)
    {
        __log_mesg("error: gige_set_exposure fail [%x]\n", nRet);
        return EXIT_FAILURE;
    }
    gigeobj.exposure = exposure;
    return EXIT_SUCCESS;
}

int gige_fps_manual_control_enable(bool enabled)
{
    int nRet = MV_CC_SetBoolValue(gigeobj.handle, "AcquisitionFrameRateEnable", enabled);
    if (MV_OK != nRet)
    {
        __log_mesg("error: gige_fps_manual_control_enable fail [%x]\n", nRet);
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}

int gige_get_external_trigger_mode(int* enabled)
{
    int ret = 0;
    ret = MV_CC_GetEnumValue(gigeobj.handle, "TriggerMode", *enabled);
    if (MV_OK != ret) {
        __log_mesg("error: gige_get_external_trigger_mode fail [%x]\n", ret);
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}

int gige_set_external_trigger_mode(int enabled)
{
    int ret = 0;
    ret = gige_fps_manual_control_enable(!enabled);
    if (ret == EXIT_FAILURE) {
        return ret;
    }
    ret = MV_CC_SetEnumValue(gigeobj.handle, "TriggerMode", enabled);
    if (MV_OK != ret) {
        __log_mesg("error: gige_set_external_trigger_mode fail [%x]\n", ret);
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}

int gige_get_width()
{
    return gigeobj.width;
}

int gige_get_height()
{
    return gigeobj.height;
}

int gige_get_image(unsigned char** data, int* pwidth, int* pheight)
{
    *pwidth = gigeobj.width;
    *pheight = gigeobj.height;
    *data = (unsigned char*)malloc(gigeobj.frame_size);
    char* tail = 0;
    if (gigeobj.rb == NULL) {
        //__log_mesg("gige camera not connected.\n");
        free(*data);
        *data = NULL;
        return EXIT_FAILURE;
    }
    tail = (char*)ringbuf_memcpy_from(*data, gigeobj.rb, gigeobj.frame_size);
    //__log_mesg("Tail addr %p\n", tail);
    if (tail == 0) {
        free(*data);
        *data = NULL;
    }
    return tail? EXIT_SUCCESS : EXIT_FAILURE;
}

void gige_free_image(unsigned char* data)
{
    if (data) {
        free(data);
        data = 0;
    }
}

void gige_test()
{
    /**
     * Repeat open and close
     * Create a thread to get frame data. Test image data is validity.
     * when grabbing you can unplug net cable and plug to
     * test reconnecting function.
     */
    while(1) {

        if (gige_open()){
            __log_mesg("gige_open failed.\n");
        }

        unsigned char* data = 0;
        int width = 0;
        int height = 0;
        do {
            int ret = gige_get_image(&data, &width, &height);
            if (ret == 1) {
                __log_mesg("get image failed.\n");
            }
            __log_mesg("w=%d h=%d data=%p\n", width, height, data);
            usleep(25000);
            gige_free_image(data);
        }while(1);

        __log_mesg("press to close grab\n");
        getchar();

        if (gige_close()) {
            __log_mesg("gige_close failed.\n");
        }

        __log_mesg("press to start grab\n");

        getchar();
    }
}